#include "aimer_filter/AimerFilter.hpp"

namespace wmj{
AimerFilter::AimerFilter(std::string name) : rclcpp::Node(name)
{
    setParam(AIMERFILTER_CFG);

    priority = m_priority_init;
    detect_num = 0;
    aimer_id = -1;
    status_sub = this->create_subscription<base_interfaces::msg::KinematicStatusOfAll>("/Top_status_of_all", 1, std::bind(&AimerFilter::status_cb, this, std::placeholders::_1));
    bt_sub = this->create_subscription<base_interfaces::msg::BtAimer>("/BtAimer", 1, std::bind(&AimerFilter::bt_cb, this, std::placeholders::_1));
    status_pub_left = this->create_publisher<base_interfaces::msg::KinematicStatus>("/left/Top_status", 1);
    status_pub_right = this->create_publisher<base_interfaces::msg::KinematicStatus>("/right/Top_status", 1);
    roi_pub = this->create_publisher<base_interfaces::msg::Roi>("/my_roi", 1);
    scan_pub = this->create_publisher<base_interfaces::msg::ScanCtrlInfo>("ScanCtrlInfo", 1);
}

void AimerFilter::setParam(const std::string &file_path)
{
    cv::FileStorage fs(file_path, cv::FileStorage::READ);
    fs["debug"] >> debug;
    fs["distance_threshold_min"] >> m_distance_threshold_min;
    fs["distance_threshold_max"] >> m_distance_threshold_max;
    fs["distance_threshold_ignore"] >> m_distance_threshold_ignore;
    fs["detect_num_threshold"] >> m_dectect_num_threshold;
    fs["priority_init"] >> m_priority_init;
    for (auto node: fs["mapping"])
    {
        int id, index;
        node["id"] >> id;
        node["index"] >> index;
        mapping[id] = index;
    }
    fs.release();
}

void AimerFilter::reset()
{
    if (debug)
    {
        std::cout << "RESET!" << std::endl;
    }
    aimer_id = -1;
    priority = m_priority_init;
    detect_num = 0;

    base_interfaces::msg::KinematicStatus msg;
    msg.enable = false;
    msg.is_track = false;
    status_pub_left->publish(msg);
    status_pub_right->publish(msg);

    m_should_scan ? publishScanMode(0) : publishScanMode(4);
    /*-------- publish roi here ----------*/
}

void AimerFilter::status_cb(const base_interfaces::msg::KinematicStatusOfAll & msg)
{
    std::cout << "recv-------------" << std::endl;
    int target_id = -1;
    if (getEnableIndex(msg.status))
    {
        if (debug)
        {
            std::cout << "status_index_within_range: ";
            for (auto i : status_index_within_range)
            {
                std::cout << i << " ";
            }
            std::cout << std::endl;
        }
        if(getTargetId(msg.status, status_index_within_range, target_id))
        {
            if (debug)
            {
                std::cout << "target_id: " << target_id << std::endl;
                std::cout << "aimer_id: " << aimer_id << std::endl;
	        }
            if (target_id == aimer_id && target_id != -1)
            {
                detect_num++;
                if (detect_num > m_dectect_num_threshold)
                {
                    //TODO: 优先级线性提升
                    if (!updatePriority(target_id))
                    {
                        reset();
                        return;
                    }
                    detect_num = 0;
                }
            }
            else
            {
                aimer_id = target_id;
                priority = m_priority_init;
                detect_num = 1;
            }
        }
        else
        {
            reset();
            return;
        }
    }
    else
    {
        priority = m_priority_init;

        std::vector<int> index_track;
        if (getTrackIndex(msg.status, index_track))
        {
            if (debug)
            {
                std::cout << "index_track: ";
                for (auto i : index_track)
                {
                    std::cout << i << " ";
                }
                std::cout << std::endl;
            }
            if (getTargetId(msg.status, index_track, target_id))
            {
                if (debug)
                {
                    std::cout << "target_id: " << target_id << std::endl;
                }
                aimer_id = target_id;
            }
            else
            {
                reset();
                return;
            }

        }
        else
        {
            reset();
            return;
        }
    }

    base_interfaces::msg::KinematicStatus kinematic_model[3];
    int detect_id_num = msg.status.size() / 3;
    for (int i = 0; i < 3; i++)
    {
        if (mapping.count(target_id))
        {
            kinematic_model[i] = msg.status[mapping[target_id] + i * detect_id_num];
            if (debug)
            {
                std::cout << "kinematic[" << i << "].enable: " << kinematic_model[i].enable << std::endl;
            }
        }
        else
        {
            std::cout << "error target id: " << target_id << std::endl;
            reset();
            return;
        }
    }

    publishScanMode(4);

    if (kinematic_model[0].enable && kinematic_model[1].enable && kinematic_model[0].id !=1 )
    {
        /*-------- publish roi here ----------*/
        /*-------- publish roi here ----------*/
        status_pub_left->publish(kinematic_model[0]);
        status_pub_right->publish(kinematic_model[1]);
    }
    else if (kinematic_model[0].enable && kinematic_model[0].id !=1 )
    {
        /*-------- publish roi here ----------*/
        status_pub_left->publish(kinematic_model[0]);
        status_pub_right->publish(kinematic_model[0]);
    }
    else if (kinematic_model[1].enable && kinematic_model[1].id !=1 )
    {
        /*-------- publish roi here ----------*/
        status_pub_left->publish(kinematic_model[1]);
        status_pub_right->publish(kinematic_model[1]);
    }
    else if (kinematic_model[2].enable && kinematic_model[2].id !=1 )
    {
        status_pub_left->publish(kinematic_model[2]);
        status_pub_right->publish(kinematic_model[2]);
    }
    else if (kinematic_model[0].is_track && kinematic_model[1].is_track && kinematic_model[0].id !=1 )
    {
        /*-------- publish roi here ----------*/
        status_pub_left->publish(kinematic_model[0]);
        status_pub_right->publish(kinematic_model[1]);
    }
    else if (kinematic_model[0].is_track && kinematic_model[0].id !=1 )
    {
        status_pub_left->publish(kinematic_model[0]);
        status_pub_right->publish(kinematic_model[0]);
    }
    else if (kinematic_model[1].is_track && kinematic_model[1].id !=1 )
    {
        status_pub_left->publish(kinematic_model[1]);
        status_pub_right->publish(kinematic_model[1]);
    }
    else if (kinematic_model[2].is_track && kinematic_model[2].id !=1 )
    {
        status_pub_left->publish(kinematic_model[2]);
        status_pub_right->publish(kinematic_model[2]);
    }
    else
    {
        std::cout << "Error! No target!" << std::endl;
        reset();
    }
    return;
}

bool AimerFilter::getEnableIndex(std::vector<base_interfaces::msg::KinematicStatus> status)
{
    std::vector<int> index; // 存放模型下标
    for (auto i = 0; i < status.size(); i++)
    {
        std::vector<int>::iterator it = std::find(status_index_within_range.begin(), status_index_within_range.end(), i);
        bool found = (it != status_index_within_range.end());   // 查找下标为i的模型有没有被记录过
        if (status[i].enable)
        {
            // 如果下标i之前没有被记录过，机器人位置在m_distance_threshold_min之内，则记录下标i
            if (!found && getDistance(cv::Point2d(status[i].center.x, status[i].center.y)) < m_distance_threshold_min)
            {
                status_index_within_range.push_back(i);
            }
            // 如果下标i之前被记录过，机器人位置在m_distance_threshold_max之外，则去除下标i
            if (found && getDistance(cv::Point2d(status[i].center.x, status[i].center.y)) > m_distance_threshold_max)
            {
                status_index_within_range.erase(it);
            }
            // 下标i的模型存在且在考虑的阈值之内，就记录至index中
            if (getDistance(cv::Point2d(status[i].center.x, status[i].center.y)) < m_distance_threshold_ignore)
            {
                index.push_back(i);
            }
        }
        else
        {
            // 如果原来被记录过，但是现在没了，说明机器人寄了或者润了，就把该下标从status_index_within_range去除了
            if (found)
            {
                status_index_within_range.erase(it);
            }
        }
    }
    // 如果在阈值范围内不存在机器人
    if (status_index_within_range.size() == 0)
    {
        // 记录建好的模型下标
        if (index.size() > 0)
        {
            status_index_within_range = index;
            return true;
        }
        else    // 没有建好的ekf模型
        {
            std::cout << "there is no model!" << std::endl;
            return false;
        }
    }
    else    // 近距离阈值之内存在机器人
    {
        return true;
    }
}

bool AimerFilter::getTrackIndex(std::vector<base_interfaces::msg::KinematicStatus> status, std::vector<int> &index_track)
{
    for (auto i = 0; i < status.size(); i++)
    {
        // 如果扫描到机器人
        if (status[i].is_track)
        {
            // 如果装甲板距离在阈值之内就记录下标
            if (status[i].armors.size() > 0)
            {
                double armor_pos_x = status[i].armors[0].armor_pose.position.x;
                double armor_pos_y = status[i].armors[0].armor_pose.position.y;
                if (getDistance(cv::Point2d(armor_pos_x, armor_pos_y)) < m_distance_threshold_ignore)
                {
                    index_track.push_back(i);
                }
            }
        }
    }
    // 如果在阈值之内不存在装甲板
    if (index_track.size() == 0)
    {
        std::cout << "there is no armor!" << std::endl;
        return false;
    }
    else    // 考虑装甲板的阈值之内存在装甲板
    {
        return true;
    }
}

bool AimerFilter::getTargetId(std::vector<base_interfaces::msg::KinematicStatus> status, std::vector<int> index_track, int &target_id)
{
    // 模型下标转化为id
    std::vector<int> id;
    for (auto index : index_track)
    {
        // id是否已经被存储
        std::vector<int>::iterator it = std::find(id.begin(), id.end(), status[index].id);
        // id是否在priority中
        std::vector<int>::iterator it_priority = std::find(priority.begin(), priority.end(), status[index].id);
        if (it == id.end() && it_priority != priority.end())    // 符合两个条件
        {
            id.push_back(status[index].id);
        }
    }
    if (debug)
    {
        std::cout << "id: ";
        for (auto id_ : id)
        {
            std::cout << id_ << " ";
        }
        std::cout << std::endl;
    }
    if (id.size() > 0)
    {
        target_id = findHighestPriorityId(id);
        return true;
    }
    else
    {
        std::cout << "id.size() == 0, not have id" << std::endl;
        target_id = -1;
        return false;
    }
}

int AimerFilter::findHighestPriorityId(std::vector<int> id)
{
    if (debug)
    {
        std::cout << "priority: ";
        for(int i = 0; i < priority.size(); i++)
            std::cout << priority[i] << " ";
        std::cout << std::endl;
    }
    std::map<int, int> key; // 根据id做优先级映射
    for (auto i = 0; i < this->priority.size(); ++i) {
        key[this->priority[i]] = this->priority.size() - i;  // 优先级高的赋予更大的值
    }
    // 寻找id向量中优先级最大元素
    auto maxPriorityId = std::max_element(id.begin(), id.end(), [&key](int id1, int id2){return key[id1] < key[id2];});
    return *maxPriorityId;
}

bool AimerFilter::updatePriority(int id)
{
    for (int i = 0; i < priority.size(); i++)
    {
        if (priority[i] == id)
        // updatePriority
        {
            if (i == 0) // 已经是最高优先级
            {
                return true;
            }
            int index = i;
            // 提高优先级
            bool flag = (priority[index - 1] == 3 || priority[index - 1] == 4 || priority[index - 1] == 5);
            if (flag)
            {
                while (flag && index > 0)
                {
                    std::swap(priority[index], priority[index - 1]);
                    index--;
                    if (index > 0)
                    {
                        flag = (priority[index - 1] == 3 || priority[index - 1] == 4 || priority[index - 1] == 5);
                    }
                }
            }
            else
            {
                std::swap(priority[index], priority[index - 1]);
            }
            return true;
        }
    }
    std::cout << "updatePriority error, can't find id!" << std::endl;
    return false;
}

void AimerFilter::publishRoi(cv::Rect2f roi, std::string frame_id)
{
    base_interfaces::msg::Roi roi_msg;
    roi_msg.roi_x = roi.x;
    roi_msg.roi_y = roi.y;
    roi_msg.roi_width = roi.width;
    roi_msg.roi_height = roi.height;

    roi_msg.header.frame_id = frame_id;
    roi_msg.header.stamp = this->now();
    roi_pub->publish(roi_msg);
}

void AimerFilter::publishScanMode(int scan_mode)
{
    base_interfaces::msg::ScanCtrlInfo scan_info;
    scan_info.scan_mode = scan_mode;
    scan_pub->publish(scan_info);
}

void AimerFilter::bt_cb(const base_interfaces::msg::BtAimer & msg)
{
    if (msg.id_queue.size() != 0)
    {
        m_priority_init.clear();
        for(int i = 0; i < msg.id_queue.size(); i++){
            if (msg.id_queue[i] != 1){
            m_priority_init.push_back(msg.id_queue[i]);
            }
        }
    }
    else
    {
        cv::FileStorage fs(AIMERFILTER_CFG, cv::FileStorage::READ);
        fs["priority_init"] >> m_priority_init;
        fs.release();
    }

    m_should_scan = msg.scan_is_open;
}

} //namespace wmj

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<wmj::AimerFilter>("aimer_filter");
  /* 运行节点，并检测状态*/
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}